/*
 * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
 *
 * This file is part of SeMoLearning.
 *
 * SeMoLearning is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * SeMoLearning is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with SeMoLearning.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <utilite/UEventsManager.h>
#include <utilite/ULogger.h>
#include <utilite/UTimer.h>
#include <utilite/UConversion.h>
#include <utilite/UProcessInfo.h>
#include <utilite/UMath.h>
#include <utilite/UStl.h>
#include <utilite/UConversion.h>
#include <utilite/UColorTable.h>

#include "semol/core/Memory.h"
#include "semol/core/Node.h"
#include "semol/core/Parameters.h"
#include "semol/core/SemolEvent.h"
#include "semol/core/Sensor.h"
#include "semol/core/DBDriver.h"
#include "semol/core/DBTrash.h"
#include "GraphNode.h"
#include "semol/core/SensoryMemory.h"

#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/core/core.hpp>

#include <iostream>


namespace semol {

const int Memory::kIdStart = 1;
const int Memory::kIdVirtual = -1;
const int Memory::kIdInvalid = 0;

Memory::Memory(const ParametersMap & parameters) :
	_dbDriver(new DBDriver(parameters)),
	_dbTrash(new DBTrash(_dbDriver)),
	_rawDataKept(Parameters::defaultMemRawDataKept()),
	_maxStMemSize(Parameters::defaultMemMaxStMemSize()),
	_recentWmRatio(Parameters::defaultMemRecentWmRatio()),
	_sensoryAttentionDecreasingRate(Parameters::defaultMemAttentionDecreasingRate()),
	_stmSizeById(Parameters::defaultMemStmSizeById()),

	_sensoryMem(new SensoryMemory(parameters)),

	_idCount(kIdStart),
	_lastNode(0),
	_lastLoopClosureId(0),
	_memoryChanged(false),
	_signaturesAdded(0)
{
	this->setSimilarityMeans(Parameters::defaultMemRehearsalMeans());
	this->setSimilarityStds(Parameters::defaultMemRehearsalStds());
	UASSERT(_similarityMeans.size() == Sensor::kTypeEnd);
	UASSERT(_similarityStds.size() == Sensor::kTypeEnd);

	this->parseParameters(parameters);
}

bool Memory::init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters)
{
	UEventsManager::post(new SemolEventInit(SemolEventInit::kInitializing));
	UDEBUG("");
	this->parseParameters(parameters);

	UEventsManager::post(new SemolEventInit("Clearing memory..."));
	this->clear();
	UEventsManager::post(new SemolEventInit("Clearing memory, done!"));

	// Assuming trash thread is not running... see clear()
	if(_dbDriver->isConnected())
	{
		UEventsManager::post(new SemolEventInit("Closing database connection..."));
		_dbDriver->closeConnection();
		UEventsManager::post(new SemolEventInit("Closing database connection, done!"));
	}

	bool success = false;

	UEventsManager::post(new SemolEventInit(std::string("Connecting to database ") + dbUrl + "..."));
	if(_dbDriver->openConnection(dbUrl, dbOverwritten))
	{
		success = true;
		UEventsManager::post(new SemolEventInit(std::string("Connecting to database ") + dbUrl + ", done!"));

		// Load the last working memory...
		UEventsManager::post(new SemolEventInit(std::string("Loading last signatures...")));
		std::list<Node*> dbSignatures;
		dbSignatures = _dbDriver->loadLastNodes();
		int lastId = 0;
		if(dbSignatures.size())
		{
			lastId = (*dbSignatures.rbegin())->id();
		}
		for(std::list<Node*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
		{
			_signatures.insert(std::pair<int, Node *>((*iter)->id(), *iter));
			if( (!_stmSizeById && (int)_stMem.size() <= _maxStMemSize) ||
				(_stmSizeById && (*iter)->id() >= lastId - _maxStMemSize) )
			{
				_stMem.insert((*iter)->id());
			}
			else
			{
				_workingMem.insert((*iter)->id());
			}
		}
		UEventsManager::post(new SemolEventInit(std::string("Loading last signatures, done! (") + uNumber2Str(int(_signatures.size())) + " loaded)"));

		// Assign the last signature
		if(_stMem.size()>0)
		{
			_lastNode = uValue(_signatures, *_stMem.rbegin(), (Node*)0);
			if(_lastNode)
			{
				_lastNodeSensors = _lastNode->getSensors();
			}
		}

		// Last id
		_idCount = _dbDriver->getLastNodeId();
		_idCount += 1;
	}
	else
	{
		UEventsManager::post(new SemolEventInit(SemolEventInit::kError, std::string("Connecting to database ") + dbUrl + ", path is invalid!"));
	}

	_sensoryAttentionWeights.clear();

	if(success)
	{
		// Now load the sensors if we have a connection
		if(_dbDriver->isConnected())
		{
			UEventsManager::post(new SemolEventInit(std::string("Loading sensors...")));
			_sensoryMem->add(_dbDriver->loadLastSensors());
			_sensoryMem->setLastSensorId(_dbDriver->getLastSensorId());
			UDEBUG("%d sensors loaded!", _sensoryMem->size());
			UEventsManager::post(new SemolEventInit(std::string("Loading sensors, done! (") + uNumber2Str(int(_sensoryMem->size())) + " loaded)"));
		}

		// Link sensors to loaded signatures
		Node * node;
		const std::map<int, Node *> & signatures = this->getSignatures();
		for(std::map<int, Node *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
		{
			node = this->_getNode(i->first);
			if(node)
			{
				for(std::vector<int>::const_iterator iter=node->getSensors().begin();
					iter!= node->getSensors().end();
					++iter)
				{
					_sensoryMem->addSensorRef(*iter, node->id());
				}
			}
		}
		if(_sensoryMem->getNotRefSensors().size() != 0)
		{
			UDEBUG("total sensors=%d, _unusedSensors=%d", _sensoryMem->size(), _sensoryMem->getNotRefSensors().size());
			for(std::set<int>::iterator iter=_sensoryMem->getNotRefSensors().begin(); iter!=_sensoryMem->getNotRefSensors().end(); ++iter)
			{
				UDEBUG("unused sensor %d", *iter);
			}
		}
	}

	UDEBUG("ids start with %d", _idCount);
	UEventsManager::post(new SemolEventInit(SemolEventInit::kInitialized));
	return success;
}

Memory::~Memory()
{
	UDEBUG("");
	if(!_memoryChanged)
	{
		_dbDriver->closeConnection();
		this->clear();
	}
	else
	{
		UDEBUG("");
		this->clear();
		_dbDriver->closeConnection();
	}
	delete _dbDriver;
	delete _dbTrash;
	delete _sensoryMem;
}

void Memory::parseParameters(const ParametersMap & parameters)
{
	UDEBUG("");
	ParametersMap::const_iterator iter;
	if((iter=parameters.find(Parameters::kMemRehearsalMeans())) != parameters.end())
	{
		this->setSimilarityMeans((*iter).second);
	}
	if((iter=parameters.find(Parameters::kMemRehearsalStds())) != parameters.end())
	{
		this->setSimilarityStds((*iter).second);
	}

	Parameters::parse(parameters, Parameters::kMemRawDataKept(), _rawDataKept);
	Parameters::parse(parameters, Parameters::kMemStmSizeById(), _stmSizeById);

	if((iter=parameters.find(Parameters::kMemMaxStMemSize())) != parameters.end())
	{
		this->setMaxStMemSize(std::atoi((*iter).second.c_str()));
	}
	if((iter=parameters.find(Parameters::kMemRecentWmRatio())) != parameters.end())
	{
		this->setRecentWmRatio(std::atof((*iter).second.c_str()));
	}
	if((iter=parameters.find(Parameters::kMemAttentionDecreasingRate())) != parameters.end())
	{
		this->setSensoryAttentionDecreasingRate(std::atof((*iter).second.c_str()));
	}
	_dbDriver->parseParameters(parameters);

	_sensoryMem->parseParameters(parameters);
}

void Memory::preUpdate()
{
	_signaturesAdded = 0;
	//this->cleanUnusedSensors();
}

bool Memory::update(const std::list<SensorRaw> & sensors, const std::list<Actuator> & actuators, Statistics & stats)
{
	UDEBUG("");
	UTimer timer;
	UTimer totalTimer;
	timer.start();
	float t;

	//============================================================
	// Pre update...
	//============================================================
	UDEBUG("pre-updating...");
	this->preUpdate();
	t=timer.ticks()*1000;
	stats.addStatistic(Statistics::kTimingMemory_pre_update(), t);
	UDEBUG("time preUpdate=%f ms", t);

	//============================================================
	//
	//============================================================

	//============================================================
	// Create a node with the sensors received.
	//============================================================
	Node * node = this->createNode(this->getNextId(), sensors);
	stats.addStatistic(Statistics::kMemorySensory_memory_size(), _sensoryMem->size());

	// It will be added to the short-term memory, no need to delete it...
	this->addNodeToSTM(node, actuators);

	_lastNode = node;
	_lastNodeSensors = node->getSensors();

	if(_lastLoopClosureId == 0 && !node->isEmpty())
	{
		// If not set use the new one added
		_lastLoopClosureId = node->id();
	}

	t=timer.ticks()*1000;
	stats.addStatistic(Statistics::kTimingMemory_node_creation(), t);
	UDEBUG("time creating node=%f ms", t);

	//============================================================
	// Rehearsal step...
	// Compare with the X last signatures. If different, add this
	// signature like a parent to the memory tree, otherwise add
	// it as a child to the similar signature.
	//============================================================
	this->rehearsal(node, stats);
	t=timer.ticks()*1000;
	stats.addStatistic(Statistics::kTimingMemory_weight_update(), t);
	UDEBUG("time weight update (rehearsal)=%f ms", t);

	//============================================================
	// Transfer the oldest signature of the short-term memory to the working memory
	//============================================================
	while(_stMem.size() &&
		  ((!_stmSizeById && (int)_stMem.size() > _maxStMemSize) || (_stmSizeById && *_stMem.begin() < _lastNode->id() - _maxStMemSize)))
	{
		UDEBUG("Inserting node %d from STM in WM...", *_stMem.begin());
		_workingMem.insert(_workingMem.end(), *_stMem.begin());
		_stMem.erase(*_stMem.begin());
		++_signaturesAdded;
	}

	while((int)_historyIds.size() > _maxStMemSize)
	{
		_historyIds.pop_back();
	}

	if(!_memoryChanged)
	{
		_memoryChanged = true;
	}

	UDEBUG("totalTimer = %fs", totalTimer.ticks());

	return true;
}

void Memory::setSimilarityMeans(const std::string & rehearsalMeans)
{
	std::list<std::string> strValues  = uSplit(rehearsalMeans, ' ');
	std::vector<float> tmpValues;
	bool ok = true;
	for(std::list<std::string>::const_iterator iter=strValues.begin(); iter!=strValues.end(); ++iter)
	{
		float value = std::atof(iter->c_str());
		if(value>=0 && value <=1)
		{
			tmpValues.push_back(value);
		}
		else
		{
			ok = false;
			break;
		}
	}
	if(ok)
	{
		UASSERT(tmpValues.size() == Sensor::kTypeEnd);
		_similarityMeans = tmpValues;
	}
	else
	{
		ULOGGER_ERROR("Error parsing rehearsalMeans \"%s\" (must be numbers between 0 and 1, and %d numbers)", rehearsalMeans.c_str(), Sensor::kTypeEnd);
	}
}

void Memory::setSimilarityStds(const std::string & rehearsalStds)
{
	std::list<std::string> strValues  = uSplit(rehearsalStds, ' ');
	std::vector<float> tmpValues;
	bool ok = true;
	for(std::list<std::string>::const_iterator iter=strValues.begin(); iter!=strValues.end(); ++iter)
	{
		float value = std::atof(iter->c_str());
		if(value>=0)
		{
			tmpValues.push_back(value);
		}
		else
		{
			ok = false;
			break;
		}
	}
	if(ok && tmpValues.size() == Sensor::kTypeEnd)
	{
		_similarityStds = tmpValues;
	}
	else
	{
		ULOGGER_ERROR("Error parsing rehearsalStds \"%s\" (must be numbers >= 0 and %d numbers)", rehearsalStds.c_str(), Sensor::kTypeEnd);
	}
}

void Memory::setMaxStMemSize(unsigned int maxStMemSize)
{
	if(maxStMemSize == 0)
	{
		ULOGGER_ERROR("maxStMemSize=%d (must be > 0)", maxStMemSize);
	}
	else
	{
		_maxStMemSize = maxStMemSize;
	}
}

void Memory::setRecentWmRatio(float recentWmRatio)
{
	if(recentWmRatio < 0 || recentWmRatio >= 1)
	{
		ULOGGER_ERROR("recentWmRatio=%f (must be >= 0 and < 1)", recentWmRatio);
	}
	else
	{
		_recentWmRatio = recentWmRatio;
	}
}

void Memory::setSensoryAttentionDecreasingRate(float rate)
{
	if(rate < 0 || rate >= 1)
	{
		ULOGGER_ERROR("sensoryAttentionDecreasingRate=%f (must be >= 0 and < 1)", rate);
	}
	else
	{
		_sensoryAttentionDecreasingRate = rate;
	}
}

void Memory::addNodeToSTM(Node * node, const std::list<Actuator> & actuators)
{
	UTimer timer;
	// add node on top of the short-term memory
	if(node)
	{
		UDEBUG("adding %d with actions=%d", node->id(), (int)actuators.size());
		for(std::list<Actuator>::const_iterator iter = actuators.begin(); iter!=actuators.end(); ++iter)
		{
			if(ULogger::level() == ULogger::kDebug)
			{
				std::cout << "A=" << iter->data() << std::endl;
			}
		}
		// Update neighbors
		if(_stMem.size())
		{
			_signatures.at(*_stMem.rbegin())->addNeighbor(NeighborLink(node->id(), _historyIds, actuators, node->id()));
			// actions are not backward compatible, so set null actions
			node->addNeighbor(NeighborLink(*_stMem.rbegin()));
			UDEBUG("Min STM id = %d", *_stMem.begin());
		}

		_signatures.insert(_signatures.end(), std::pair<int, Node *>(node->id(), node));
		_stMem.insert(_stMem.end(), node->id());

		_historyIds.push_front(node->getSensors());

		/*for(std::list<std::vector<int> >::iterator iter=_historyIds.begin(); iter!=_historyIds.end(); ++iter)
		{
			if(iter->size())
			{
				for(unsigned int i=0; i<iter->size(); ++i)
				{
					printf(" %d", iter->at(i));
				}
				printf("\n");
			}
			else
			{
				printf("---\n");
			}
		}*/
	}

	UDEBUG("time = %fs", timer.ticks());
}

void Memory::addSignatureToWm(Node * signature)
{
	if(signature)
	{
		UDEBUG("Inserting node %d in WM...", signature->id());
		_workingMem.insert(signature->id());
		_signatures.insert(std::pair<int, Node*>(signature->id(), signature));
		++_signaturesAdded;
	}
	else
	{
		UERROR("Signature is null ?!?");
	}
}

const Node * Memory::getNode(int id) const
{
	return _getNode(id);
}

Node * Memory::_getNode(int id) const
{
	return uValue(_signatures, id, (Node*)0);
}

const Sensor * Memory::getSensor(int sensorId) const
{
	return _sensoryMem->getSensor(sensorId);
}

std::list<const NeighborLink *> Memory::getNeighborLinks(int signatureId, bool ignoreNeighborByLoopClosure, bool onlyWithActions) const
{
	std::list<const NeighborLink *> links;
	Node * sTop = uValue(_signatures, signatureId, (Node*)0);
	if(sTop)
	{
		std::list<Node *> loops;
		loops.push_back(sTop);
		while(loops.size())
		{
			Node * s = *loops.begin();
			loops.pop_front();
			if(s)
			{
				const NeighborsMultiMap & neighbors = s->getNeighbors();
				for(NeighborsMultiMap::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
				{
					if(!onlyWithActions || (onlyWithActions && iter->second.actuatorId() && iter->second.actuators().size()))
					{
						links.push_back(&iter->second);
					}
					else if(iter->second.actuatorId() || iter->second.actuators().size())
					{
						UWARN("link %d with aid=%d (actuators=%d) is inconsistent", iter->first, iter->second.actuatorId(), iter->second.actuators().size());
					}
				}
				if(!ignoreNeighborByLoopClosure)
				{
					const std::set<int> & loopIds = s->getLoopClosureIds();
					for(std::set<int>::const_iterator iter = loopIds.begin(); iter!=loopIds.end(); ++iter)
					{
						if(*iter > 0 && _stMem.find(*iter) == _stMem.end())
						{
							loops.push_back(uValue(_signatures, *iter, (Node*)0));
						}
					}
				}
			}
		}

		if(!ignoreNeighborByLoopClosure)
		{
			// Check for child loop closure ids
			const std::set<int> & childTopIds = sTop->getChildLoopClosureIds();
			for(std::set<int>::const_iterator iter = childTopIds.begin(); iter!=childTopIds.end(); ++iter)
			{
				if(*iter > 0 && _stMem.find(*iter) == _stMem.end())
				{
					loops.push_back(uValue(_signatures, *iter, (Node*)0));
				}
			}
			while(loops.size())
			{
				Node * s = *loops.begin();
				loops.pop_front();
				if(s)
				{
					const NeighborsMultiMap & neighbors = s->getNeighbors();
					for(NeighborsMultiMap::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
					{
						if(!onlyWithActions || (onlyWithActions && iter->second.actuators().size()))
						{
							links.push_back(&iter->second);
						}
					}
					const std::set<int> & childIds = s->getChildLoopClosureIds();
					for(std::set<int>::const_iterator iter = childIds.begin(); iter!=childIds.end(); ++iter)
					{
						if(*iter > 0 && _stMem.find(*iter) == _stMem.end())
						{
							loops.push_back(uValue(_signatures, *iter, (Node*)0));
						}
					}
				}
			}
		}
	}
	else
	{
		UWARN("Cannot find signature %d in memory", signatureId);
	}
	return links;
}

bool Memory::isInNeighborLimits(int id, const std::set<int> & aids) const
{
	if(aids.size() == 0)
	{
		return true;
	}
	const Node * s = this->getNode(id);
	std::set<int> idsToCheck;
	if(s)
	{
		// add action ids
		idsToCheck = s->getNeighborsAll(); // s->getAids();
		// add children
		idsToCheck.insert(s->getChildLoopClosureIds().begin(), s->getChildLoopClosureIds().end());
	}
	// add id
	idsToCheck.insert(id);

	int maxActionsChecked = 99999;
	int actionsChecked = 0;
	int lastAction = 0;

	// look if an action id is in the limits
	std::set<int>::const_iterator searchIter;
	for(std::set<int>::const_reverse_iterator iter = aids.rbegin();
		iter!=aids.rend() && actionsChecked < maxActionsChecked;
		++iter)
	{
		if(lastAction == 0 ||
		   (lastAction && (*iter < lastAction - this->getMaxStMemSize() || *iter > lastAction + this->getMaxStMemSize())))
		{
			++actionsChecked;
		}
		lastAction = *iter;
		searchIter = idsToCheck.lower_bound(*iter - this->getMaxStMemSize());
		//if(searchIter != idsToCheck.end() && *searchIter <= *iter + this->getMaxStMemSize())
		if(searchIter != idsToCheck.end() && *searchIter <= *iter + 10)
		{
			//UDEBUG("%d (aid=%d) is in limits <%d,%d>",
			//		id,
			//		*idsToCheck.lower_bound(*iter - this->getMaxStMemSize()),
			//		*iter - this->getMaxStMemSize(),
			//		*iter + this->getMaxStMemSize());
			return true;
		}
	}

	return false;
}

// return map<Id,Margin>, including signatureId
// maxCheckedInDatabase = -1 means no limit to check in database (default)
// maxCheckedInDatabase = 0 means don't check in database, returned ids all exist in WM
std::map<int, int> Memory::getNeighborsId(int signatureId,
		unsigned int margin,
		int maxCheckedInDatabase, // default -1 (no limit)
		bool onlyWithActions, // default false
		bool incrementMarginOnLoop, // default false
		bool ignoreLoopIds, // default false
		double * dbAccessTime
		) const
{
	//UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
	if(dbAccessTime)
	{
		*dbAccessTime = 0;
	}
	std::map<int, int> ids;
	if(signatureId<=0)
	{
		return ids;
	}
	int nbLoadedFromDb = 0;
	std::list<int> curentMarginList;
	std::set<int> currentMargin;
	std::set<int> nextMargin;
	if(maxCheckedInDatabase!=0 || this->getNode(signatureId))
	{
		nextMargin.insert(signatureId);
	}
	unsigned int m = 0;
	while(m < margin && nextMargin.size())
	{
		curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
		nextMargin.clear();
		// first pass: count number of node in current margin in database
		for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end();++jter)
		{
			if(!uContains(ids, *jter))
			{
				const Node * s = this->getNode(*jter);
				if(!s)
				{
					++nbLoadedFromDb;
				}
			}
		}

		for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
		{
			if(ids.insert(std::pair<int, int>(*jter, m)).second)
			{
				//UDEBUG("Added %d with margin %d", *jter, m);
				// Look up in STM/WM if all ids are here, if not... load them from the database
				const Node * s = this->getNode(*jter);
				std::set<int> tmpNeighborIds;
				std::set<int> tmpLoopClosureIds;
				std::set<int> tmpChildLoopClosureIds;
				const std::set<int> * neighborIds = &tmpNeighborIds;
				const std::set<int> * loopClosureIds = &tmpLoopClosureIds;
				const std::set<int> * childLoopClosureIds = &tmpChildLoopClosureIds;
				if(s)
				{
					if(onlyWithActions)
					{
						neighborIds = &s->getNeighborsWithActuators();
					}
					else
					{
						neighborIds = &s->getNeighborsAll();
					}
					if(!ignoreLoopIds)
					{
						loopClosureIds = &s->getLoopClosureIds();
						childLoopClosureIds = &s->getChildLoopClosureIds();
					}
				}
				else if(maxCheckedInDatabase == -1 || (maxCheckedInDatabase > 0 && nbLoadedFromDb < maxCheckedInDatabase))
				{
					UTimer timer;
					tmpNeighborIds = _dbDriver->getNeighborIds(*jter, onlyWithActions);
					if(!ignoreLoopIds)
					{
						_dbDriver->getLoopClosureIds(*jter, tmpLoopClosureIds, tmpChildLoopClosureIds);
					}
					if(dbAccessTime)
					{
						*dbAccessTime += timer.getElapsedTime();
					}
				}

				// Neighbor links
				for(std::set<int>::const_iterator iter=neighborIds->begin(); iter!=neighborIds->end(); ++iter)
				{
					if( (ignoreLoopIds || (loopClosureIds->find(*iter) == loopClosureIds->end() && childLoopClosureIds->find(*iter) == childLoopClosureIds->end())) &&
						!uContains(ids, *iter) &&
						nextMargin.find(*iter) == nextMargin.end())
					{
						if(maxCheckedInDatabase!=0 || this->getNode(*iter))
						{
							nextMargin.insert(*iter);
							//UDEBUG("next of %d + %d", *jter, *iter);
						}
					}
				}

				// Parent links
				for(std::set<int>::const_iterator iter=loopClosureIds->begin(); iter!=loopClosureIds->end(); ++iter)
				{
					if( *iter && !uContains(ids, *iter)/* && isInNeighborLimits(*iter, limits)*/)
					{
						if(maxCheckedInDatabase!=0 || this->getNode(*iter))
						{
							if(incrementMarginOnLoop)
							{
								nextMargin.insert(*iter);
								//UDEBUG("next of %d + %d", *jter, *iter);
							}
							else
							{
								if(currentMargin.insert(*iter).second)
								{
									const Node * s = this->getNode(*iter);
									if(!s)
									{
										// update db count because it's on current margin
										++nbLoadedFromDb;
									}
									curentMarginList.push_back(*iter);
									//UDEBUG("current of %d + %d", *jter, *iter);
								}
							}
						}
					}
				}

				//Child links
				for(std::set<int>::const_iterator iter=childLoopClosureIds->begin(); iter!=childLoopClosureIds->end(); ++iter)
				{
					if( *iter && !uContains(ids, *iter)/* && isInNeighborLimits(*iter, limits)*/)
					{
						if(maxCheckedInDatabase!=0 || this->getNode(*iter))
						{
							if(incrementMarginOnLoop)
							{
								nextMargin.insert(*iter);
								//UDEBUG("next of %d + %d", *jter, *iter);
							}
							else
							{
								if(currentMargin.insert(*iter).second)
								{
									const Node * s = this->getNode(*iter);
									if(!s)
									{
										// update db count because it's on current margin
										++nbLoadedFromDb;
									}
									curentMarginList.push_back(*iter);
									//UDEBUG("current of %d + %d", *jter, *iter);
								}
							}
						}
					}
				}
			}
		}
		++m;
	}
	return ids;
}

void Memory::getLoopClosureIds(int signatureId, std::set<int> & loopClosureIds, std::set<int> & childLoopClosureIds, bool lookInDatabase) const
{
	const Node * s = this->getNode(signatureId);
	loopClosureIds.clear();
	childLoopClosureIds.clear();
	if(s)
	{
		loopClosureIds = s->getLoopClosureIds();
		childLoopClosureIds = s->getChildLoopClosureIds();
	}
	else if(lookInDatabase)
	{
		_dbDriver->getLoopClosureIds(signatureId, loopClosureIds, childLoopClosureIds);
	}
}

int Memory::getNextId()
{
	return _idCount++;
}

int Memory::getDatabaseMemoryUsed() const
{
	return _dbDriver->getMemoryUsed()/(1024*1024); //Byte to MB
}

double Memory::getDbSavingTime() const
{
	return _dbTrash->getEmptyTrashesTime();
}

std::set<int> Memory::getAllSignatureIds() const
{
	std::set<int> ids = _dbDriver->getAllNodeIds();
	for(std::map<int, Node*>::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter)
	{
		ids.insert(iter->first);
	}
	return ids;
}

void Memory::clear()
{
	UDEBUG("");

	_dbTrash->emptyTrashes();
	_dbTrash->join();

	// Save some stats to the db, save only when the mem is not empty
	if(_stMem.size() || _workingMem.size())
	{
		unsigned int memSize = _workingMem.size() + _stMem.size();
		if(_workingMem.size() && *_workingMem.begin() < 0)
		{
			--memSize;
		}
		if(memSize != _signatures.size())
		{
			// this is only a safe check...not supposed to occur.
			ULOGGER_ERROR("The number of signatures don't match! _workingMem=%d, _stMem=%d, _signatures=%d", _workingMem.size(), _stMem.size(), _signatures.size());
		}

		UDEBUG("Adding statistics after run...");
		_dbDriver->addStatistics(memSize, _lastNode?_lastNode->id():0, UProcessInfo::getMemoryUsage(), this->getDatabaseMemoryUsed());
	}
	UDEBUG("");

	//Get the tree root (parents)
	std::map<int, Node*> mem = _signatures;
	for(std::map<int, Node *>::iterator i=mem.begin(); i!=mem.end(); ++i)
	{
		if(i->second)
		{
			UDEBUG("deleting from the working and the short-time memory: %d", i->first);
			this->moveToTrash(i->second);
		}
	}

	if(_workingMem.size() != 0 && !(_workingMem.size()==1 && *_workingMem.begin() == kIdVirtual))
	{
		ULOGGER_ERROR("_workingMem must be empty here, size=%d", _workingMem.size());
	}
	_workingMem.clear();
	_workingMem.insert(kIdVirtual);
	if(_stMem.size() != 0)
	{
		ULOGGER_ERROR("_stMem must be empty here, size=%d", _stMem.size());
	}
	_stMem.clear();

	if(_sensoryMem->size() - _sensoryMem->getNotRefSensors().size() > 0)
	{
		UERROR("Sensors (size=%d, notRef=%d) should be all unreferenced here...",
				_sensoryMem->size(), _sensoryMem->getNotRefSensors().size());
	}
	cleanUnusedSensors();

	_sensoryMem->clear();

	UDEBUG("");
	_lastNode = 0;
	_lastNodeSensors.clear();
	_lastLoopClosureId = 0;
	_idCount = kIdStart;
	_memoryChanged = false;
	_historyIds.clear();

	// Wait until the db trash has finished cleaning the memory
	_dbTrash->emptyTrashes();
}

void Memory::cleanUnusedSensors()
{
	std::set<int> notRefSensors = _sensoryMem->getNotRefSensors();
	for(std::set<int>::iterator iter=notRefSensors.begin(); iter!=notRefSensors.end(); ++iter)
	{
		Sensor * s = _sensoryMem->remove(*iter);
		if(s)
		{
			if(s->totalReferences()!=0)
			{
				UERROR("Sensor %d still have %d references!", *iter, s->totalReferences());
			}

			_dbTrash->asyncSave(s);
		}
	}
	UINFO("%d sensors removed...", notRefSensors.size());
}

std::vector<float> Memory::compareHistory(const std::list<std::vector<int> > & sensorIdsA,
										  const std::list<std::vector<int> > & sensorIdsB)
{
	unsigned int numSensors = this->getSensoryAttentionWeights().size();
	if(numSensors == 0)
	{
		return std::vector<float>();
	}
	std::vector<float> sim(numSensors, 0.0f);
	int nbCompared = 0;
	int nbNotFoundA = 0;
	int nbNotFoundB = 0;
	std::list<std::vector<int> >::const_iterator iterA = sensorIdsA.begin();
	std::list<std::vector<int> >::const_iterator iterB = sensorIdsB.begin();
	int countBuffered = 0;
	int countFullComparison = 0;
	UTimer timer;
	// iterate from more recent to old one
	for(; iterA != sensorIdsA.end() && iterB != sensorIdsB.end(); ++iterA, ++iterB)
	{
		if(iterA->size() == 0 || iterB->size() == 0)
		{
			// break when empty sensors are found (happens with initial history)
			break;
		}
		UASSERT_MSG(iterA->size() == numSensors && iterB->size() == numSensors,
				uFormat("numSensors=%d, iterA->size()=%d, iterB->size()=%d", numSensors, iterA->size(), iterB->size()).c_str());
		++nbCompared;
		bool bufferedLookup = false;
		for(unsigned j=0; j<numSensors; ++j)
		{
			sim[j] += _sensoryMem->compare(iterA->at(j), iterB->at(j), &bufferedLookup);
		}
		bufferedLookup?++countBuffered:++countFullComparison;
	}

	int total = (int)(sensorIdsA.size()>sensorIdsB.size()?sensorIdsA.size():sensorIdsB.size());
	if(total)
	{
		for(unsigned int j=0; j<sim.size(); ++j)
		{
			sim[j] /= float(total);
		}
	}
	UDEBUG("time=%fs, nbCompared=%d, countBuffered=%d, countFullComparison=%d, similarityMap=%d",
			timer.ticks(), nbCompared, countBuffered, countFullComparison, _sensoryMem->getSimilaritySensorMapSize());
	if(nbNotFoundA || nbNotFoundB)
	{
		UDEBUG("Not found A(%d)=%d, Not found B(%d)=%d, nbCompared=%d", (int)sensorIdsA.size(), nbNotFoundA, (int)sensorIdsB.size(), nbNotFoundB, nbCompared);
	}
	return sim;
}

std::vector<float> Memory::compareSensors(const std::vector<int> & sensorA, const std::vector<int> & sensorB)
{
	std::vector<float> similarities;
	if(sensorA.size() == sensorB.size()) //Compatible
	{
		similarities = std::vector<float>(sensorA.size(), 0.0f);
		// compare sensors
		std::vector<int>::const_iterator iterA = sensorA.begin();
		std::vector<int>::const_iterator iterB = sensorB.begin();
		int j=0;
		while(iterA != sensorA.end() && iterB != sensorB.end())
		{
			similarities[j] = _sensoryMem->compare(*iterA, *iterB);

			++iterA;
			++iterB;
			++j;
		}

		if(ULogger::level() == ULogger::kDebug)
		{
			std::string str;
			for(unsigned int i=0; i<similarities.size(); ++i)
			{
				str.append(uFormat("%f", similarities[i]));
				if(i<similarities.size()-1)
				{
					str.append(", ");
				}
			}
			//UDEBUG("similarities (%d vs %d) = [%s]", this->id(), s->id(), str.c_str());
		}
	}
	else if(!sensorA.empty() && !sensorB.empty())
	{
		UWARN("Not compatible nodes : nb sensors A=%d B=%d", (int)sensorA.size(), (int)sensorB.size());
	}

	if(similarities.size() == 0)
	{
		UFATAL("similarities.size() == 0, (size=%d vs %d)", sensorA.size(), sensorB.size());
	}
	return similarities;
}

/**
 * Compute the likelihood of the node with some others in the memory.
 * If an error occurs, the result is empty.
 */
std::map<int, std::vector<float> > Memory::computeLikelihoods(const std::vector<int> & sensors, const std::list<int> & ids)
{
	UTimer timer;
	timer.start();
	std::map<int, std::vector<float> > likelihood;

	if(!sensors.size())
	{
		ULOGGER_ERROR("The signature is null");
		return likelihood;
	}
	else if(ids.empty())
	{
		UWARN("ids list is empty");
		return likelihood;
	}

	for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
	{
		std::vector<float> similarities(_sensoryAttentionWeights.size(), 0.0f);
		if(*iter > 0)
		{
			const Node * sB = this->getNode(*iter);
			if(!sB)
			{
				UFATAL("Signature %d not found in WM ?!?", *iter);
			}
			similarities = this->compareSensors(sensors, sB->getSensors());
		}
		//UDEBUG("Insert likelihood %d size %d _sensoryAttentionWeights.size()=%d", *iter, similarities.size(), _sensoryAttentionWeights.size());
		likelihood.insert(likelihood.end(), std::pair<int, std::vector<float> >(*iter, similarities));
	}

	UDEBUG("compute likelihood... %f s", timer.ticks());
	return likelihood;
}

// Weights of the signatures in the working memory <signature id, weight>
std::map<int, int> Memory::getWeights() const
{
	std::map<int, int> weights;
	for(std::set<int>::const_iterator iter=_workingMem.begin(); iter!=_workingMem.end(); ++iter)
	{
		if(*iter > 0)
		{
			const Node * s = this->getNode(*iter);
			if(!s)
			{
				UFATAL("Location %d must exist in memory", *iter);
			}
			weights.insert(weights.end(), std::make_pair(*iter, s->getWeight()));
		}
		else
		{
			weights.insert(weights.end(), std::make_pair(*iter, -1));
		}
	}
	return weights;
}

int Memory::forget(const std::set<int> & ignoredIds)
{
	UDEBUG("");
	// Remove one more than total added during the iteration
	std::list<Node *> signatures = getRemovableSignatures(_signaturesAdded+1, ignoredIds);
	for(std::list<Node *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
	{
		// When a signature is deleted, it notifies the memory
		// and it is removed from the memory list
		this->moveToTrash(*iter);
	}
	UDEBUG("signaturesRemoved=%d, _signaturesAdded=%d", (int)signatures.size(), _signaturesAdded);
	return (int)signatures.size();
}


int Memory::cleanup(const std::list<int> & ignoredIds)
{
	UDEBUG("");
	int signaturesRemoved = 0;

	// bad signature
	if(_lastNode->isEmpty())
	{
		moveToTrash(_lastNode);
		++signaturesRemoved;
	}

	return signaturesRemoved;
}

void Memory::startTrashThread()
{
	UDEBUG("");
	_dbTrash->emptyTrashes(true);
}

void Memory::joinTrashThread()
{
	UDEBUG("");
	_dbTrash->join();
	UDEBUG("");
}

Node * Memory::createNode(int id, const std::list<SensorRaw> & rawSensors)
{
	UDEBUG("");
	UTimer timer;
	timer.start();
	UTimer timerDetails;
	timerDetails.start();
	std::vector<int> sensorIds;

	// Process all sensors
	for(std::list<SensorRaw>::const_iterator iter = rawSensors.begin(); iter!=rawSensors.end(); ++iter)
	{
		sensorIds.push_back(_sensoryMem->add(*iter));
		_sensoryMem->addSensorRef(sensorIds.back(), id);
		UDEBUG("Sensor %d added to node %d", sensorIds.back(), id);
	}

	UDEBUG("Images = %d", _sensoryMem->getSensors()[Sensor::kTypeImage].size());
	UDEBUG("AudioFreqSqrdMagns = %d", _sensoryMem->getSensors()[Sensor::kTypeAudioFreqSqrdMagn].size());
	UDEBUG("Twists = %d", _sensoryMem->getSensors()[Sensor::kTypeTwist].size());

	UDEBUG("time new signature (id=%d) %fs", id, timer.ticks());
	return new Node(id, sensorIds);
}

std::set<int> Memory::reactivateNodes(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess)
{
	// get the nodes, if not in the working memory, they
	// will be loaded from the database

	UDEBUG("");
	UTimer timer;
	std::list<int> idsToLoad;
	std::map<int, int>::iterator wmIter;
	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
	{
		if(!this->getNode(*i) && !uContains(idsToLoad, *i))
		{
			if(!maxLoaded || idsToLoad.size() < maxLoaded)
			{
				UDEBUG("id = %d", *i);
				idsToLoad.push_back(*i);
			}
			else
			{
				UDEBUG("id = %d", *i);
				break;
			}
		}
		else
		{
			UDEBUG("id = %d", *i);
		}
	}

	UDEBUG("idsToLoad = %d", idsToLoad.size());

	std::list<Node *> reactivatedNodes;
	reactivatedNodes = _dbTrash->loadNodes(idsToLoad);
	timeDbAccess = timer.getElapsedTime();

	//Check if there are sensors not in memory, if so... load them
	std::set<int> sensorsToLoad;
	for(std::list<Node *>::iterator i=reactivatedNodes.begin(); i!=reactivatedNodes.end(); ++i)
	{
		Node * node = *i;
		for(std::vector<int>::const_iterator iter=node->getSensors().begin();
			iter!= node->getSensors().end();
			++iter)
		{
			if(!_sensoryMem->contains(*iter) && *iter > 0)
			{
				sensorsToLoad.insert(*iter);
			}
		}
	}
	if(sensorsToLoad.size())
	{
		_sensoryMem->add(_dbTrash->loadSensors(sensorsToLoad));
	}

	//Add references to sensors...
	for(std::list<Node *>::iterator i=reactivatedNodes.begin(); i!=reactivatedNodes.end(); ++i)
	{
		Node * node = *i;
		for(std::vector<int>::const_iterator iter=node->getSensors().begin();
			iter!= node->getSensors().end();
			++iter)
		{
			_sensoryMem->addSensorRef(*iter, node->id());
		}

		//append to working memory
		this->addSignatureToWm(*i);
	}
	UDEBUG("time = %fs", timer.ticks());
	return std::set<int>(idsToLoad.begin(), idsToLoad.end());
}

class WeightIdKey
{
public:
	WeightIdKey(int w, int i) :
		weight(w),
		id(i) {}
	bool operator<(const WeightIdKey & k) const
	{
		if(weight < k.weight)
		{
			return true;
		}
		else if(weight == k.weight)
		{
			if(id < k.id)
			{
				return true;
			}
		}
		return false;
	}
	int weight, id;
};
std::list<Node *> Memory::getRemovableSignatures(int count, const std::set<int> & ignoredIds)
{
	//UDEBUG("");
	std::list<Node *> removableSignatures;
	std::map<WeightIdKey, Node *> signatureMap;

	// Find the last index to check...
	const std::set<int> & wm = _workingMem;
	UDEBUG("mem.size()=%d, ignoredIds.size()=%d", wm.size(), ignoredIds.size());

	if(wm.size())
	{
		int recentWmMaxSize = _recentWmRatio * float(wm.size());
		bool recentWmImmunized = false;
		// look for the position of the lastLoopClosureId in WM
		int currentRecentWmSize = 0;
		if(_lastLoopClosureId > 0 && _stMem.find(_lastLoopClosureId) == _stMem.end())
		{
			// If set, it must be in WM
			std::set<int>::const_iterator iter = _workingMem.find(_lastLoopClosureId);
			while(iter != _workingMem.end())
			{
				++currentRecentWmSize;
				++iter;
			}
			if(currentRecentWmSize>1 && currentRecentWmSize < recentWmMaxSize)
			{
				recentWmImmunized = true;
			}
			else if(currentRecentWmSize == 0 && _workingMem.size() > 1)
			{
				UERROR("Last loop closure id not found in WM (%d)", _lastLoopClosureId);
			}
			UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastLoopClosureId);
		}

		// Ignore neighbor of the last location in STM (for neighbor links redirection issue during Rehearsal).
		Node * lastInSTM = 0;
		if(_stMem.size())
		{
			lastInSTM = _signatures.at(*_stMem.begin());
		}

		for(std::set<int>::const_iterator memIter = wm.begin(); memIter != wm.end(); ++memIter)
		{
			if( (recentWmImmunized && *memIter > _lastLoopClosureId) ||
				*memIter == _lastLoopClosureId)
			{
				// ignore recent memory
			}
			else if(*memIter > 0 && ignoredIds.find(*memIter) == ignoredIds.end() && (!lastInSTM || !lastInSTM->hasNeighbor(*memIter)))
			{
				Node * s = this->_getNode(*memIter);
				if(s)
				{
					// Its loop closures must not be in STM to be removable, rehearsal issue
					bool foundInSTM = false;
					for(std::set<int>::const_iterator iter = s->getLoopClosureIds().begin(); iter!=s->getLoopClosureIds().end(); ++iter)
					{
						if(_stMem.find(*iter) != _stMem.end())
						{
							UDEBUG("Ignored %d because it has a parent (%d) in STM", s->id(), *iter);
							foundInSTM = true;
							break;
						}
					}
					// In loopClosureMerged mode, ignore if a child is in WM
					if(!foundInSTM)
					{
						for(std::set<int>::const_iterator iter = s->getChildLoopClosureIds().begin(); iter!=s->getChildLoopClosureIds().end(); ++iter)
						{
							if(isInWM(*iter))
							{
								UDEBUG("Ignored %d because it has a child (%d) in WM", s->id(), *iter);
								foundInSTM = true;
								break;
							}
						}
					}
					// Its neighbors must not be in STM to be removable, rehearsal issue
					if(!foundInSTM)
					{
						for(std::set<int>::iterator iter = _stMem.begin(); iter!=_stMem.end(); ++iter)
						{
							if(s->hasNeighbor(*iter))
							{
								UDEBUG("Ignored %d because it has a neighbor (%d) in STM", s->id(), *iter);
								foundInSTM = true;
								break;
							}
						}
					}
					if(!foundInSTM)
					{
						// less weighted signature priority to be transferred
						signatureMap.insert(std::make_pair(WeightIdKey(s->getWeight(), s->id()), s));
					}
				}
				else
				{
					ULOGGER_ERROR("Not supposed to occur!!!");
				}
			}
			else
			{
				//UDEBUG("Ignoring id %d", memIter->first);
			}
		}

		int recentWmCount = 0;
		std::set<int> addedSignatures;
		// make the list of removable signatures
		// Criteria : Weight -> ID
		UDEBUG("signatureMap.size()=%d", (int)signatureMap.size());
		for(std::map<WeightIdKey, Node*>::iterator iter=signatureMap.begin();
			iter!=signatureMap.end();
			++iter)
		{
			bool removable = true;
			if(removable)
			{
				if(!recentWmImmunized)
				{

					UDEBUG("weight=%d, id=%d, lcCount=%d, lcId=%d, childId=%d",
							iter->first.weight,
							iter->second->id(),
							int(iter->second->getLoopClosureIds().size()),
							iter->second->getLoopClosureIds().size()?*iter->second->getLoopClosureIds().rbegin():0,
							iter->second->getChildLoopClosureIds().size()?*iter->second->getChildLoopClosureIds().rbegin():0);
					removableSignatures.push_back(iter->second);
					addedSignatures.insert(iter->second->id());

					if(iter->second->id() > _lastLoopClosureId)
					{
						++recentWmCount;
						if(currentRecentWmSize - recentWmCount < recentWmMaxSize)
						{
							UDEBUG("switched recentWmImmunized");
							recentWmImmunized = true;
						}
					}
				}
				else if(iter->second->id() < _lastLoopClosureId)
				{
					UDEBUG("weight=%d, id=%d, lcCount=%d, lcId=%d, childId=%d",
							iter->first.weight,
							iter->second->id(),
							int(iter->second->getLoopClosureIds().size()),
							iter->second->getLoopClosureIds().size()?*iter->second->getLoopClosureIds().rbegin():0,
							iter->second->getChildLoopClosureIds().size()?*iter->second->getChildLoopClosureIds().rbegin():0);
					removableSignatures.push_back(iter->second);
					addedSignatures.insert(iter->second->id());
				}
				if(removableSignatures.size() >= (unsigned int)count)
				{
					break;
				}
			}
		}
	}
	else
	{
		ULOGGER_WARN("not enough signatures to get an old one...");
	}
	return removableSignatures;
}

void Memory::moveToTrash(Node * node)
{
	UINFO("id=%d", node?node->id():0);
	if(node)
	{
		//remove references to sensors
		for(std::vector<int>::const_iterator iter = node->getSensors().begin();
			iter!=node->getSensors().end();
			++iter)
		{
			_sensoryMem->removeSensorRef(*iter, node->id());
		}

		_workingMem.erase(node->id());
		_stMem.erase(node->id());
		_signatures.erase(node->id());

		if(_lastNode == node)
		{
			_lastNode = 0;
			_lastNodeSensors.clear();
		}

		if(_memoryChanged &&
			node->id()>0)
		{
			_dbTrash->asyncSave(node);
		}
		else
		{
			delete node;
		}
	}
}

const Node * Memory::getLastNode() const
{
	UDEBUG("");
	return _lastNode;
}

const std::vector<int> Memory::getLastNodeSensors() const
{
	UDEBUG("");
	return _lastNodeSensors;
}

bool Memory::rehearsalMerge(int oldId, int newId)
{
	UINFO("old=%d, new=%d", oldId, newId);
	Node * oldNode = _getNode(oldId);
	Node * newNode = _getNode(newId);
	if(oldNode && newNode)
	{
		// just to keep track of merged nodes
		oldNode->addLoopClosureId(newNode->id());
		UDEBUG("Add loop closure link between %d and %d", oldNode->id(), newNode->id());

		// During rehearsal in STM...
		// Here we merge the new location with the old one,
		// redirecting all neighbor links to new location.
		if(_lastLoopClosureId == oldNode->id())
		{
			_lastLoopClosureId = newNode->id();
		}

		// update weight only when this is not direct neighbor
		if(oldId == newId-1)
		{
			newNode->setWeight(newNode->getWeight() + oldNode->getWeight());
		}
		else
		{
			newNode->setWeight(newNode->getWeight() + 1 + oldNode->getWeight());
		}


		UDEBUG("");
		// redirect neighbor links
		newNode->changeNeighborIds(oldNode->id(), newNode->id());

		NeighborsMultiMap neighbors = oldNode->getNeighbors(); //copy
		bool allSameIdsAdded = false;
		for(NeighborsMultiMap::const_iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter)
		{
			const NeighborLink & link = iter->second;
			// Forward links have baseIds set, Backward links don't have actions
			if(link.toId() == newNode->id() && link.sensorHistoryIds().size())
			{
				// Limit the number of self references to STM size (historyIds size)
				bool allSameIds = link.isSensorHistorySameIds();
				if(!allSameIds || (allSameIds && !allSameIdsAdded))
				{
					newNode->addNeighbor(link); // self reference
					if(allSameIds && !allSameIdsAdded)
					{
						allSameIdsAdded = true;
					}
				}
				else if (link.actuators().size())
				{
					// Show warning when actions are used.
					UWARN("Ignored self reference link because base ids are all the same. (id=%d)", newNode->id());
				}
			}
			else if(link.toId() != newNode->id() && link.toId() != oldNode->id())
			{
				Node * node = this->_getNode(link.toId());
				if(node)
				{
					// modify neighbor "from"
					node->changeNeighborIds(oldNode->id(), newNode->id());

					if(link.sensorHistoryIds().size() || !newNode->hasNeighbor(link.toId()))
					{
						newNode->addNeighbor(link);
					}

					// only keep the more recent link between the two nodes
					// TODO why??
					//node->keepUniqueLink(newNode->id());
				}
				else
				{
					UERROR("Didn't find neighbor %d of %d in RAM...", link.toId(), oldNode->id());
				}
				oldNode->removeNeighbor(link.toId(), link.actuatorId());
			}
			else if(link.toId() == oldNode->id() && link.actuators().size())
			{
				NeighborLink newLink(newId, link.sensorHistoryIds(), link.actuators(), link.actuatorId());
				newNode->addNeighbor(newLink);
				oldNode->removeNeighbor(link.toId(), link.actuatorId());
			}

			if(link.toId() == newNode->id())
			{
				oldNode->removeNeighbor(link.toId(), link.actuatorId());
			}
		}


		// redirect child loop closure links
		const std::set<int> & childIds = oldNode->getChildLoopClosureIds();
		for(std::set<int>::const_iterator iter = childIds.begin(); iter!=childIds.end(); ++iter)
		{
			if(*iter == newNode->id())
			{
				UERROR("");
			}
			newNode->addChildLoopClosureId(*iter);
			Node * node = _getNode(*iter);
			if(node)
			{
				UDEBUG("changed child Loop closure %d from %d to %d", *iter, oldNode->id(), newNode->id());
				node->removeLoopClosureId(oldNode->id());
				node->addLoopClosureId(newNode->id());
			}
			else
			{
				UERROR("A location (%d, child of %d) in WM/STM cannot be transferred if its loop closure id is in STM", *iter, oldNode->id());
			}
		}

		// keep old sensor ids in new node
		//remove references to sensors
		for(std::vector<int>::const_iterator iter = newNode->getSensors().begin();
			iter!=newNode->getSensors().end();
			++iter)
		{
			_sensoryMem->removeSensorRef(*iter, newNode->id());
		}
		for(unsigned int i=0;i<oldNode->getSensors().size(); ++i)
		{
			if(oldNode->getSensors()[i] > 0)
			{
				newNode->changeSensorsRef(newNode->getSensors()[i], oldNode->getSensors()[i]);
				_historyIds.front()[i] = oldNode->getSensors()[i];
			}
		}
		for(std::vector<int>::const_iterator iter = newNode->getSensors().begin();
			iter!=newNode->getSensors().end();
			++iter)
		{
			_sensoryMem->addSensorRef(*iter, newNode->id());
		}


		// remove old location
		moveToTrash(oldNode);
		return true;
	}
	else
	{
		if(!newNode)
		{
			UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, newId);
		}
		if(!oldNode)
		{
			UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId);
		}
	}
	return false;
}

bool Memory::addLoopClosureLink(int oldId, int newId)
{
	UINFO("old=%d, new=%d", oldId, newId);
	Node * oldNode = _getNode(oldId);
	Node * newNode = _getNode(newId);
	if(oldNode && newNode)
	{
		const std::set<int> & oldLoopclosureIds = oldNode->getLoopClosureIds();
		if(oldLoopclosureIds.size() && oldLoopclosureIds.find(newNode->id()) != oldLoopclosureIds.end())
		{
			// do nothing, already merged
			UINFO("already merged, old=%d, new=%d", oldId, newId);
			return true;
		}
		/*else if(oldLoopclosureIds.size())
		{
			if(oldLoopclosureIds.size() > 1)
			{
				UWARN("Multiple (%d) parents detected for node %d", oldLoopclosureIds.size(), oldNode->id());
			}
			for(std::set<int>::const_reverse_iterator iter = oldLoopclosureIds.rbegin(); iter!=oldLoopclosureIds.rend(); ++iter)
			{
				// Cannot be the first in STM because of the neighbor redirection issue.
				if(*iter != *_stMem.begin())
				{
					// If there is already a loop closure with a location, loop/rehearse with it.
					return addLoopClosureLink(*iter, newId);
				}
			}
			UWARN("Parent loop closure of %d not found in RAM (or is the last int STM), reject loop closure", oldNode->id());
			return false;
		}*/

		// if already neighbors, don't add loop closure link
		/*if(newNode->getNeighbors().find(oldNode->id()) != newNode->getNeighbors().end())
		{
			UWARN("Already neighbors (newLoop = %d, oldLoop=%d)", newNode->id(), oldNode->id());
			return true;
		}*/

		// Set loop closure link
		oldNode->addLoopClosureId(newNode->id());
		UDEBUG("Add loop closure link between %d and %d", oldNode->id(), newNode->id());

		// During loop closure in WM
		newNode->addChildLoopClosureId(oldNode->id());
		_lastLoopClosureId = newNode->id();
		newNode->setWeight(newNode->getWeight() + oldNode->getWeight());
		oldNode->setWeight(0);

		return true;
	}
	else
	{
		if(!newNode)
		{
			UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, newId);
		}
		if(!oldNode)
		{
			UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId);
		}
	}
	return false;
}

void Memory::dumpMemory(std::string directory) const
{
	UDEBUG("");
	this->dumpSignatures((directory + "DumpMemorySign.txt").c_str());
	this->dumpMemoryTree((directory + "DumpMemoryTree.txt").c_str());
}

void Memory::dumpMemoryTree(const char * fileNameTree) const
{
	FILE* foutTree = 0;
	#ifdef _MSC_VER
		fopen_s(&foutTree, fileNameTree, "w");
	#else
		foutTree = fopen(fileNameTree, "w");
	#endif

	if(foutTree)
	{
		fprintf(foutTree, "SignatureID Weight NbLoopClosureIds LoopClosureIds... NbChildLoopClosureIds ChildLoopClosureIds...\n");

		for(std::map<int, Node *>::const_iterator i=_signatures.begin(); i!=_signatures.end(); ++i)
		{
			fprintf(foutTree, "%d %d", i->first, i->second->getWeight());

			const std::set<int> & loopIds = i->second->getLoopClosureIds();
			fprintf(foutTree, " %d", (int)loopIds.size());
			for(std::set<int>::const_iterator j=loopIds.begin(); j!=loopIds.end(); ++j)
			{
				fprintf(foutTree, " %d", *j);
			}

			const std::set<int> & childIds = i->second->getChildLoopClosureIds();
			fprintf(foutTree, " %d", (int)childIds.size());
			for(std::set<int>::const_iterator j=childIds.begin(); j!=childIds.end(); ++j)
			{
				fprintf(foutTree, " %d", *j);
			}

			fprintf(foutTree, "\n");
		}

		fclose(foutTree);
	}

}

bool Memory::rehearsal(Node * node, Statistics & stats)
{
	if(node->getNeighbors().size() != 1)
	{
		return false;
	}

	UTimer timer;

	//============================================================
	// Compare with all nodes in STM
	//============================================================
	std::list<int> ids;
	if(_stMem.size() > 2)
	{
		// ignore the oldest (which is linked to nodes in WM) and the new one
		ids.insert(ids.end(), ++_stMem.begin(), --_stMem.end());
	}
	else
	{
		return false;
	}

	std::map<int, std::vector<float> > likelihoods = this->computeLikelihoods(node->getSensors(), ids);

	std::vector<float> independentMax(node->getSensors().size());
	std::vector<int> independentId(node->getSensors().size(), 0);
	std::vector<float> combinedMax(node->getSensors().size());
	int combinedId = 0;

	for(std::map<int, std::vector<float> >::iterator iter=likelihoods.begin(); iter!=likelihoods.end(); ++iter)
	{
		if(iter->second.size() == independentMax.size())
		{
			bool allOver = true;
			for(unsigned int i=0; i<independentMax.size(); ++i)
			{
				if(independentId[i] == 0 || iter->second[i] >= independentMax[i])
				{
					independentId[i] = iter->first;
					independentMax[i] = iter->second[i];
				}
				if(combinedId != 0 && iter->second[i] < combinedMax[i])
				{
					allOver = false;
				}
			}
			if(allOver)
			{
				combinedMax = iter->second;
				combinedId = iter->first;
			}
		}
		else
		{
			UERROR("");
		}
	}

	// look if the combined maximum is over all similarity thresholds
	std::vector<float> rehearsalMeans(node->getSensors().size(), -1.0f);
	bool merge = true;
	for(unsigned int i=0; i<node->getSensors().size(); ++i)
	{
		if(node->getSensors()[i] > 0)
		{
			const Sensor * s = _sensoryMem->getSensor(node->getSensors()[i]);
			if(s && s->type() < Sensor::kTypeEnd)
			{
				rehearsalMeans[i] = _similarityMeans[s->type()];

				if(combinedMax[i] < rehearsalMeans[i])
				{
					merge=false;
				}
			}
			else
			{
				UERROR("");
			}
		}
	}

	int sensorsMerged= 0;
	if(merge)
	{
		this->rehearsalMerge(combinedId, node->id());
	}
	else
	{
		// check if there are single sensors to merge (keep old data)
		for(unsigned int i=0; i<independentMax.size(); ++i)
		{
			if(rehearsalMeans[i] >=0 && independentMax[i] >= rehearsalMeans[i])
			{
				int oldSensorId = this->getNode(independentId[i])->getSensors()[i];
				UDEBUG("Merging sensor %d to %d", node->getSensors()[i], oldSensorId);
				_sensoryMem->removeSensorRef(node->getSensors()[i], node->id());
				node->changeSensorsRef(node->getSensors()[i], oldSensorId);
				_sensoryMem->addSensorRef(node->getSensors()[i], node->id());
				++sensorsMerged;
				_historyIds.front()[i] = oldSensorId;
			}
		}
	}

	//============================================================
	// Update sensory attention weights...
	//============================================================
	this->updateSensoryAttentionWeights(combinedMax);

	//============================================================
	// statistics
	//============================================================
	stats.addStatistic(Statistics::kWeightUpdateId(), merge?combinedId:0);
	stats.addStatistic(Statistics::kWeightUpdateSensorsMerged(), (float)sensorsMerged);
	for(unsigned int i=0; i<independentMax.size(); ++i)
	{
		stats.addStatistic(uFormat("WeightUpdate/max %d/", i), independentMax[i]);
		stats.addStatistic(uFormat("WeightUpdate/maxId %d/", i), (float)independentId[i]);
		UDEBUG("sensor%d max=%f, maxId=%d", i, independentMax[i], independentId[i]);
	}
	UDEBUG("lastMerged=%d, t=%fs", merge?combinedId:0, timer.ticks());

	return merge;
}

void Memory::updateSensoryAttentionWeights(const std::vector<float> & similarities)
{
	if(similarities.size() == 0)
	{
		UERROR("similarities.size() = 0 ?!?");
		return;
	}

	_sensoryAttentionSimilarities = similarities;

	if(similarities.size() != _sensoryAttentionWeights.size() && _sensoryAttentionWeights.size())
	{
		UERROR("Similarities changed ?!?");
		_sensoryAttentionWeights.clear();
	}

	if(_sensoryAttentionWeights.size() == 0)
	{
		// initialize all weights to 1
		_sensoryAttentionWeights = std::vector<float>(similarities.size(), 1.0f);
	}
	else if(similarities.size() == 1)
	{
		// do nothing, _sensoryAttentionWeights[0] should be 1.0f;
	}
	else
	{
		for(unsigned int i=0; i<_sensoryAttentionWeights.size(); ++i)
		{
			if(_sensoryAttentionWeights[i] > 1.0f - similarities[i] || similarities[i]==0.0f)
			{
				_sensoryAttentionWeights[i] -= _sensoryAttentionDecreasingRate;
				if(_sensoryAttentionWeights[i] < 1.0f - similarities[i] && similarities[i] != 0.0f)
				{
					_sensoryAttentionWeights[i] = 1.0f - similarities[i];
				}
				else if(_sensoryAttentionWeights[i] < 0.0f)
				{
					_sensoryAttentionWeights[i] = 0.0f;
				}
			}
			else if(similarities[i]<=1.0f)
			{
				_sensoryAttentionWeights[i] = 1.0f - similarities[i];
			}
			else
			{
				UERROR("Updated failed similarities[%d]>1.0f ?!?", i);
			}
			UDEBUG("_sensoryAttentionWeights[%d] = %f", i, _sensoryAttentionWeights[i]);
		}
	}
}

float Memory::applySensoryAttention(const std::vector<float> & similarities, bool isNormalizedSim) const
{
	if(similarities.size()==1)
	{
		return similarities.front();
	}
	float sumWeights = uSum(_sensoryAttentionWeights);
	float value = isNormalizedSim?1.0f:0.0f;
	if(similarities.size() == _sensoryAttentionWeights.size())
	{
		if(sumWeights)
		{
			if(isNormalizedSim)
			{
				// values are [1,inf[
				// get max
				value=0;
				bool maxWeighting = true;
				if(maxWeighting)
				{
					for(unsigned int i=0; i<similarities.size(); ++i)
					{
						if(value < (similarities[i]-1.0f) * _sensoryAttentionWeights[i])
						{
							value = (similarities[i]-1.0f) * _sensoryAttentionWeights[i];
						}
					}
				}
				else
				{
					for(unsigned int i=0; i<similarities.size(); ++i)
					{
						value += (similarities[i]-1.0f) * _sensoryAttentionWeights[i];
					}
					value /= sumWeights;
				}
				value += 1;
			}
			else
			{
				// values are [0,1]
				sumWeights = 0.0f;
				for(unsigned int i=0; i<similarities.size(); ++i)
				{
					if(similarities[i]>=0)
					{
						sumWeights += _sensoryAttentionWeights[i];
						value += similarities[i] * _sensoryAttentionWeights[i];
						UDEBUG("iter->second[%d]=%f", i, similarities[i]);
					}
				}
				if(sumWeights)
				{
					value /= sumWeights;
				}
			}
			//UDEBUG("value=%f, sumWeights=%f", value, sumWeights);
		}
	}
	else
	{
		UERROR("Similarity sensory size (%d) must be equal to sensory attention weights (%d)", similarities.size(), _sensoryAttentionWeights.size());
	}
	return value;
}

std::map<int, float> Memory::applySensoryAttention(const std::map<int, std::vector<float> > & likelihood,
		bool isAdjustedLikelihood) const
{
	std::map<int, float> processedLikelihood;
	float value;
	for(std::map<int, std::vector<float> >::const_iterator iter = likelihood.begin(); iter!=likelihood.end();++iter)
	{
		if(iter->second.size()==0 && iter->first != kIdVirtual)
		{
			UERROR("iter->second=0?!? id=%d", iter->first);
		}
		value = this->applySensoryAttention(iter->second, isAdjustedLikelihood);
		processedLikelihood.insert(processedLikelihood.end(), std::make_pair(iter->first, value));
	}

	return processedLikelihood;
}

std::list<SensorRaw> Memory::getRawData(int nodeId) const
{
	const Node * node = this->getNode(nodeId);
	std::list<SensorRaw> data;
	if(node)
	{
		const std::vector<int> & ids = node->getSensors();
		for(std::vector<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
		{
			const Sensor * sensor = _sensoryMem->getSensor(*iter);
			if(sensor && !sensor->rawData().empty())
			{
				data.push_back(SensorRaw(sensor->type(), sensor->rawData()));
			}
		}
	}
	if(data.empty())
	{
		if(this->isRawDataKept() && _dbDriver->isConnected())
		{
			data = _dbDriver->getRawData(nodeId);
		}
	}
	return data;
}

void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
{
	if(!_dbDriver->isConnected())
	{
		UERROR("A database must must loaded first...");
		return;
	}

	if(!fileName.empty())
	{
		FILE* fout = 0;
		#ifdef _MSC_VER
			fopen_s(&fout, fileName.c_str(), "w");
		#else
			fout = fopen(fileName.c_str(), "w");
		#endif

		 if (!fout)
		 {
			 UERROR("Cannot open file %s!", fileName.c_str());
			 return;
		 }

		 if(ids.size() == 0)
		 {
			 ids = _dbDriver->getAllNodeIds();
			 UDEBUG("ids.size()=%d", ids.size());
			 for(std::map<int, Node*>::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
			 {
				 ids.insert(iter->first);
			 }
		 }

		 const char * colorG = "green";
		 const char * colorP = "pink";
		 const char * colorA = "red";
		 const char * colorB = "skyBlue";
		 const char * colorO = "darkorange1";
		 UINFO("Generating map with %d locations", ids.size());
		 fprintf(fout, "digraph G {\n");
		 for(std::set<int>::iterator i=ids.begin(); i!=ids.end(); ++i)
		 {
			 if(_signatures.find(*i) == _signatures.end())
			 {
				 int id = *i;
				 std::set<int> loopIds;
				 std::set<int> childIds;
				 _dbDriver->getLoopClosureIds(id, loopIds, childIds);
				 //don't show children
				// if(loopIds.size() == 0)
				 {
					 NeighborsMultiMap neighbors = _dbDriver->getNeighbors(id);
					 int weight =  _dbDriver->getWeight(id);
					 for(NeighborsMultiMap::iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter)
					 {
						 if(id!=iter->first)
						 {
							 int weightNeighbor =  _dbDriver->getWeight(iter->first);
							 UDEBUG("Add neighbor link from %d to %d with aid=%d", id, iter->first, iter->second.actuatorId());
							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"%d\", fontcolor=%s, fontsize=8];\n",
									 id,
									 weight,
									 iter->first,
									 weightNeighbor,
									 iter->second.actuatorId(),
									 iter->second.actuatorId()>0?colorA:colorB);
						 }
					 }

					 // Add self reference links
					 if(neighbors.count(id))
					 {
						 std::string next;
						 for(NeighborsMultiMap::iterator iter = neighbors.find(id); iter!=neighbors.end() && iter->first==id; ++iter)
						 {
							 next += uNumber2Str(iter->second.actuatorId()) + ",";
						 }
						 next.erase(next.size()-1);
						 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"%s\", fontcolor=%s, fontsize=8];\n",
								 id,
								 weight,
								 id,
								 weight,
								 next.c_str(),
								 colorO);
					 }

					 // loop closure links...
					 for(std::set<int>::iterator iter = loopIds.begin(); iter!=loopIds.end(); ++iter)
					 {
						 int weightNeighbor = 0;
						 if(_signatures.find(*iter) == _signatures.end())
						 {
							 weightNeighbor = _dbDriver->getWeight(*iter);
						 }
						 else
						 {
							 weightNeighbor = _signatures.find(*iter)->second->getWeight();
						 }
						 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
								 id,
								 weight,
								 *iter,
								 weightNeighbor,
								 colorG);
					 }
					 for(std::set<int>::iterator iter = childIds.begin(); iter!=childIds.end(); ++iter)
					 {
						 int weightNeighbor = 0;
						 if(_signatures.find(*iter) == _signatures.end())
						 {
							 weightNeighbor = _dbDriver->getWeight(*iter);
						 }
						 else
						 {
							 weightNeighbor = _signatures.find(*iter)->second->getWeight();
						 }
						 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
								 id,
								 weight,
								 *iter,
								 weightNeighbor,
								 colorP);
					 }
				 }
			 }
		 }
		 for(std::map<int, Node*>::iterator i=_signatures.begin(); i!=_signatures.end(); ++i)
		 {
			 if(ids.find(i->first) != ids.end())
			 {
				 int id = i->second->id();
				 const std::set<int> & loopIds = i->second->getLoopClosureIds();
				 //don't show children when _loopClosuresMerged is on
				 //if(!_loopClosuresMerged || (loopIds.size() == 0))
				 {
					 const NeighborsMultiMap & neighbors = i->second->getNeighbors();
					 int weight = i->second->getWeight();
					 for(NeighborsMultiMap::const_iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter)
					 {
						 //if(linksAdded.find(std::pair<int, int>(id, iter->first)) == linksAdded.end() &&
						//	linksAdded.find(std::pair<int, int>(iter->first, id)) == linksAdded.end())
						 if(id != iter->first)
						 {
							 int weightNeighbor = 0;
							 const Node * s = this->getNode(iter->first);
							 if(s)
							 {
								 weightNeighbor = s->getWeight();
							 }
							 else
							 {
								 weightNeighbor = _dbDriver->getWeight(iter->first);
							 }
							 UDEBUG("Add neighbor link from %d to %d with aid=%d", id, iter->first, iter->second.actuatorId());
							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"%d\", fontcolor=%s, fontsize=8];\n",
									 id,
									 weight,
									 iter->first,
									 weightNeighbor,
									 iter->second.actuatorId(),
									 iter->second.actuatorId()>0?colorA:colorB);
						 }
					 }
					 // Add self reference links
					 if(neighbors.count(id))
					 {
						 std::string next;
						 for(NeighborsMultiMap::const_iterator iter = neighbors.find(id); iter!=neighbors.end() && iter->first==id; ++iter)
						 {
							 next += uNumber2Str(iter->second.actuatorId()) + ",";
						 }
						 next.erase(next.size()-1);
						 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"%s\", fontcolor=%s, fontsize=8];\n",
								 id,
								 weight,
								 id,
								 weight,
								 next.c_str(),
								 colorO);
					 }

					 // loop closure link
					 for(std::set<int>::const_iterator iter = loopIds.begin(); iter!=loopIds.end(); ++iter)
					 {
						 int weightNeighbor = 0;
						 if(_signatures.find(*iter) == _signatures.end())
						 {
							 weightNeighbor = _dbDriver->getWeight(*iter);
						 }
						 else
						 {
							 weightNeighbor = _signatures.find(*iter)->second->getWeight();
						 }
						 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
								 id,
								 weight,
								 *iter,
								 weightNeighbor,
								 colorG);
					 }

					 // child loop closure link
					 const std::set<int> & childIds = i->second->getChildLoopClosureIds();
					 for(std::set<int>::const_iterator iter = childIds.begin(); iter!=childIds.end(); ++iter)
					 {
						 int weightNeighbor = 0;
						 if(_signatures.find(*iter) == _signatures.end())
						 {
							 weightNeighbor = _dbDriver->getWeight(*iter);
						 }
						 else
						 {
							 weightNeighbor = _signatures.find(*iter)->second->getWeight();
						 }
						 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
								 id,
								 weight,
								 *iter,
								 weightNeighbor,
								 colorP);
					 }
				 }
			 }
		 }
		 fprintf(fout, "}\n");
		 fclose(fout);
		 UINFO("Graph saved to \"%s\"", fileName.c_str());
	}
}

//recursive
void Memory::createGraph(GraphNode * parent, unsigned int maxDepth, const std::set<int> & endIds)
{
	if(maxDepth == 0 || !parent)
	{
		return;
	}
	std::map<int, int> neighbors = this->getNeighborsId(parent->id(), 1, -1, false, false);
	for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
	{
		if(!parent->isAncestor(iter->first))
		{
			GraphNode * n = new GraphNode(iter->first, parent);
			if(endIds.find(iter->first) == endIds.end())
			{
				this->createGraph(n, maxDepth-1, endIds);
			}
		}
	}
}

} // namespace rtabmap
